/******************************************************************
 * 文件：PID.c
 * 功能：PID控制函数
 * 日期：2018-02-16
 * 作者：zx
 * 版本：Ver.1.0 | 最初版本
 * 
 * Copyright (C) 2018 zx. All rights reserved.
*******************************************************************/
#include "PID/PID.h"
#include "TIMx/TIMx.h"

PID pid;
Error sptr;

void PID_init(void)
{
	/*设置定时器2时钟为10us,1KHz*/
	initTIMx(TIM2, 719, 100, TIM_IT_Update, DISABLE);
	initTIM2OC1(100); //对应PA15 TIM2_CH1
	//initTIM2OC2(100);	//对应PB3  TIM2_CH2

	// /*设置定时器4时钟为10us,1KHz*/
	// initTIMx(TIM4, 719, 100, TIM_IT_Update, DISABLE);
	// initTIM4OC1(100); //对应PB6  TIM4_CH1

	/* 陀螺仪默认回传速率为100HZ,10ms,所以设置定时器3时钟为15ms */
	initTIMx(TIM3, 7199, 149, TIM_IT_Update, ENABLE);
	TIM_ARRPreloadConfig(TIM3, ENABLE);

	/* 初始化PID参数 */
	pid.T = 10; //ms
	pid.P = 10;
	pid.I = 10;
	pid.D = 10;
	pid.CurrPwm = 100; //应为0~100之间，不能超过定时器的定时周期
	//pid.Now_place=getval();//getval()泛指读取函数
	pid.limit = 1000;
}
/**
 * 功能：位置式PID
 * 参数：*sptr      误差
 *      *pid        pid参数
 * 		注意：此处sptr->Current_Error为积分误差
 * 返回值：int pwm值
 */
int PID_Realize(Error *sptr, PID *pid)
{
#ifndef __Realize
#define __Realize
#endif
	int iError,																				   // 当前误差
		Realize;																			   //实际输出
	iError = pid->Target - pid->Now_place;													   // 计算当前误差
	sptr->Current_Error += pid->I * iError;													   // 误差积分
	sptr->Current_Error = sptr->Current_Error > pid->limit ? pid->limit : sptr->Current_Error; //积分限幅
	sptr->Current_Error = sptr->Current_Error < -pid->limit ? -pid->limit : sptr->Current_Error;
	Realize = pid->P * iError						  //比例P
			  + sptr->Current_Error					  //积分I
			  + pid->D * (iError - sptr->Last_Error); //微分D
	sptr->Last_Error = iError;						  // 更新上次误差
	return Realize;									  // 返回实际值
}

/**
 * 功能：增量式PID
 * 参数：*sptr      误差
 *      *pid        pid参数
 * 返回值：int 增量
 */
int PID_Increase(Error *sptr, PID *pid)
{
#ifndef __Increase
#define __Increase
#endif
	int iError,																	  //当前误差
		Increase;																  //最后得出的实际增量
	iError = pid->Target - pid->Now_place;										  //计算当前误差
	Increase = pid->P * (iError - sptr->Last_Error)								  //比例P
			   + pid->I * iError												  //积分I
			   + pid->D * (iError - 2 * sptr->Last_Error + sptr->Previous_Error); //微分D
	sptr->Previous_Error = sptr->Last_Error;									  // 更新前次误差（Ek-2）
	sptr->Last_Error = iError;													  // 更新上次误差(Ek-1)
	return Increase;															  // 返回增量
}

/**
 * 功能：PID输出函数，在定时器3中断中调用
 * 参数：void
 *   对于其他通道,分别有对应的函数名,函数格式是TIM_SetComparex(x=1/2/3/4);
 *  
 *
 *返回值：void
*/
void PID_OUT(void)
{

#ifdef __Increase
	pid.CurrPwm += PID_Increase(&sptr, &pid);
	TIM_SetCompare1(TIM2, getPeriod(TIM2) * pid.CurrPwm / 100);

#else
	pid.CurrPwm = PID_Realize(&sptr, &pid);
	TIM_SetCompare1(TIM2, getPeriod(TIM2) * pid.CurrPwm / 100);

#endif
}

